#include "handstick.hpp"

#include <cmath>
#include <cstring>
#include <iostream>
using namespace std;



/////////
//
// HandState implementation
//
/////////


HandState::HandState()
  : distance(NEUTRAL), direction(MIDDLE)
{ }


HandState::HandState(Distance dist, Direction dir)
  : distance(dist), direction(dir)
{ }


HandState HandState::create(cv::Point2f& left, cv::Point2f& right)
{
  double dist_px = dist(left, right) * 1/SCALE;
  double angle_dg = line_angle(left, right) * (-180 / PI);

  Distance distance;
  Direction direction;

  if (dist_px <= HAND_CLOSE_THRESHOLD) {
    distance = CLOSE;
  }
  else if (dist_px <= HAND_NEUTRAL_THRESHOLD) {
    distance = NEUTRAL;
  }
  else {
    distance = FAR;
  }

  if (abs(angle_dg) <= HAND_MIDDLE_THRESHOLD) {
    direction = MIDDLE;
  }
  else if (angle_dg > 0) {
    direction = LEFT;
  }
  else {
    direction = RIGHT;
  }

  return HandState(distance, direction);
}


ostream& operator<<(ostream& out, const HandState& hand_st)
{
  out << "Distance=";

  if (hand_st.distance == HandState::CLOSE)
    out << "CLOSE";
  else if (hand_st.distance == HandState::FAR)
    out << "FAR";
  else
    out << "NEUTRAL";

  out << "   Direction=";

  if (hand_st.direction == HandState::LEFT)
    out << "LEFT";
  else if (hand_st.direction == HandState::RIGHT)
    out << "RIGHT";
  else
    out << "MIDDLE";

  return out;
}



/////////
//
// WindowController implementation
//
/////////


WindowController::WindowController()
  : xdo(NULL), window(NULL)
{ }


WindowController::~WindowController()
{
  tracking_lost();
  xdo_free(xdo);
}


WindowController* WindowController::create(const string& window_title)
{
  WindowController* w_ctr = new WindowController();

  w_ctr->xdo = xdo_new(NULL);

  xdo_search_t query;
  memset(&query, 0, sizeof(xdo_search_t));

  query.winname = window_title.c_str();
  query.max_depth = -1;
  query.require = xdo_search::SEARCH_ALL;
  query.searchmask = SEARCH_NAME;
  
  Window* window_list;
  int num_windows;

  if (xdo_window_search(w_ctr->xdo, &query, &window_list, &num_windows)) {
    cerr << "A problem occured when searching for the window";
    return NULL;
  }

  w_ctr->window = window_list[0];
  return w_ctr;
}


void WindowController::tracking_started()
{
  cout << "activating window = " << window << endl;
  xdo_window_activate(xdo, window);
  xdo_window_wait_for_active(xdo, window, 1);
}


void WindowController::update(const HandState& new_st)
{
  static HandState old_st;

  // see if the user changed the distance
  if (new_st.distance != old_st.distance) switch (new_st.distance) {  
    // NEUTRAL -> CLOSE
    case HandState::CLOSE:
      xdo_keysequence_down(xdo, CURRENTWINDOW, "Down", 0);
      break;

    case HandState::NEUTRAL:
      // CLOSE -> NEUTRAL
      if (old_st.distance == HandState::CLOSE)
        xdo_keysequence_up(xdo, CURRENTWINDOW, "Down", 0);
      // FAR -> NEUTRAL
      else
        xdo_keysequence_up(xdo, CURRENTWINDOW, "Up", 0);
      break;

    // NEUTRAL -> FAR
    case HandState::FAR:
      xdo_keysequence_down(xdo, CURRENTWINDOW, "Up", 0);
      break;
      
    default:
      break;
  }

  // see if the user changed the orientation
  if (new_st.direction != old_st.direction) switch (new_st.direction) {
    // MIDDLE -> LEFT
    case HandState::LEFT:
      xdo_keysequence_down(xdo, CURRENTWINDOW, "Left", 0);
      break;

    case HandState::MIDDLE:
      // LEFT -> MIDDLE
      if (old_st.direction == HandState::LEFT)
        xdo_keysequence_up(xdo, CURRENTWINDOW, "Left", 0);
      // RIGHT -> MIDDLE
      else
        xdo_keysequence_up(xdo, CURRENTWINDOW, "Right", 0);
      break;

    // MIDDLE -> RIGHT
    case HandState::RIGHT:
      xdo_keysequence_down(xdo, CURRENTWINDOW, "Right", 0);
      break;

    default:
      break;
  }

  old_st = new_st;
}


void WindowController::tracking_lost()
{
  xdo_keysequence_up(xdo, CURRENTWINDOW, "Up", 0);
  xdo_keysequence_up(xdo, CURRENTWINDOW, "Down", 0);
  xdo_keysequence_up(xdo, CURRENTWINDOW, "Left", 0);
  xdo_keysequence_up(xdo, CURRENTWINDOW, "Right", 0);
}
